%% 2024.10.7 栅格
clc;
clear all;
close all;
warning off
%% 固定随机数种子
rng('default')
noRng=1;
rng(noRng)
%% 载入地图
global data
%% 载入地图
map=xlsread("地图03.xlsx");
%map=map(1:100,1:100);
data.mapSize=size(map);
%% 开始节点 和 途径节点
[p1,p2]=find(map==2);
start_coords = [p1,p2];
[p1,p2]=find(map==3);
aimP=[p1,p2];  % 设定了途经点
map(start_coords(1),start_coords(2))=0;
map(aimP(1),aimP(2))=0;
data.map=map;
data.Path=[start_coords;aimP];%2*2的矩阵，第一行是起始点坐标，第二行是终点坐标
%%
[p1,p2]=find(map==0);
data.node=[p1,p2];%n行2列的矩阵（自由栅格坐标矩阵），第一列是行数，第二列是列数，每一行代表一个自由栅格的坐标
data.numNode=length(data.node(:,1));% 自由栅格的数量
[p1,p2]=find(map==1);
data.B=[p1,p2];%n行2列矩阵，包含了所有障碍栅格坐标

data.D=pdist2(data.node,data.node);%计算自由栅格两两之间的距离  构成自由栅格数*自由栅格数的矩阵
% data.D(i,j)表示第i个自由栅格和第j个自由栅格之间的距离且data.D(i,j)=data.D(j,i)

[p1,p2]=find(data.D<=sqrt(2));%找到距离矩阵中小于等于根号2的元素的行索引和列索引
index=sub2ind(size(data.D),p1,p2);%把行列索引转化为一维索引，表示data.D中的一个位置
data.net=[p1,p2,data.D(index)];%将起点、终点和它们之间的距离组成的矩阵存储在 data.net 中，每一行表示一条边，其中第一列是起点索引，第二列是终点索引，第三列是起点到终点的距离
figure
hold on
% for i=1:length(data.node(:,1))
%     x=data.node(i,1);
%     y=data.node(i,2);
%     rectangle('Position',[y-0.5,x-0.5,1,1],'FaceColor','w')
% end
for i=1:length(data.B(:,1))
    x=data.B(i,1);%x对应行
    y=data.B(i,2);%y对应列
    rectangle('Position',[y-0.5,x-0.5,1,1],'FaceColor','k')
end
axis equal%matlab中坐标原点在左下角，横轴为y，纵轴为x???
title('初始地图')
%% 决策变量
lb=0;
ub=1;
dim=data.numNode;
option.lb=lb;
option.ub=ub;
option.dim=dim;
if length(option.lb)==1
    option.lb=ones(1,option.dim)*option.lb;
    option.ub=ones(1,option.dim)*option.ub;
end
option.fobj=@aimFcn_1;
option.showIter=0;
%% 算法参数设置 Parameters
% 基本参数
option.numAgent=10;        %种群个体数 size of population
option.maxIteration=10;    %最大迭代次数 maximum number of interation

option.numTrial=10;
%%
str_legend=[{'HHO'}];
algorithm=[{@myHHO}];
%% 初始化
data.F=0;
rng(noRng)
x=ones(option.numAgent,option.dim);
y=ones(option.numAgent,1);
for i=1:option.numAgent
    x(i,:)=rand(size(option.lb)).*(option.ub-option.lb)+option.lb;  %随机选自由栅格，用适应函数连起来顺便评估
    y(i)=option.fobj(x(i,:),option,data);
end
%% 使用算法求解
bestX=x;
for i=1:length(algorithm)
    if i==3
        data.F=1;
    else
        data.F=0;
    end
    rng(noRng)
    tic
    [bestY(i,:),bestX(i,:),recording(i)]=algorithm{i}(x,y,option,data);
    tt(i)=toc;
end
%% 绘制迭代曲线
figure
hold on
for i=1:length(algorithm)
    plot(recording(i).bestFit,'LineWidth',2)
end
legend(str_legend)
xlabel('t')
ylabel('目标函数值')
title('目标迭代收敛曲线')

%% 计算结果
for i=1:length(str_legend)
    if i==3
        data.F=1;
    else
        data.F=0;
    end
    str=['算法',str_legend{i}];
    [Y(i),result(i)]=option.fobj(bestX(i,:),option,data);
    drawPC(result(i),option,data,str)
end
%% DWA
% 设置动态和静态障碍物
newObstacle=[98,83;];%动态
newObstacle_R=2;%半径
newObstacle_v=[-0.25,0
    ];%运动速度、方向
newObstacle_F=[ 95,90;
                ];%静态
newObstacle_F_R=2;
for i=1:length(str_legend)
    str=['算法',str_legend{i},'+DWA'];
    path=result(i).path1(:,2:3);
    sumD=result(i).fit;
    disp(['算法',str_legend{i},'全局静态规划时间:',num2str(tt(i))])
    disp(['算法',str_legend{i},'全局静态规划长度:',num2str(sumD)])
    tic
    myDWA(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
    myDWA_3D(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
end
% 纯DWA
str=['DWA'];
path=[result(i).path1(1,2:3);result(i).path1(end,2:3)];
sumD=result(i).fit;
myDWA(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
myDWA_3D(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
% 无DWA
str=['HHO离线路线'];
path=result(i).path1(:,2:3);
sumD=result(i).fit;
myDWA0(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
myDWA_3D0(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
%% DWA-无障碍物
% 设置动态和静态障碍物
newObstacle=[];%动态
newObstacle_R=2;%半径
newObstacle_v=[];%运动速度、方向
newObstacle_F=[];%静态
newObstacle_F_R=2;
for i=1:length(str_legend)
    str=['算法',str_legend{i},'+DWA'];
    path=result(i).path1(:,2:3);
    sumD=result(i).fit;
    disp(['算法',str_legend{i},'全局静态规划时间:',num2str(tt(i))])
    disp(['算法',str_legend{i},'全局静态规划长度:',num2str(sumD)])
    myDWA(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
    myDWA_3D(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
end
% 纯DWA
str=['DWA'];
path=[result(i).path1(1,2:3);result(i).path1(end,2:3)];
sumD=result(i).fit;
myDWA(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
myDWA_3D(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
% 无DWA
str=['HHO离线路线'];
path=result(i).path1(:,2:3);
sumD=result(i).fit;
myDWA0(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);
myDWA_3D0(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str);